//#include <asm/types.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <iostream>
#include <cstring>

#include "CameraInterface.h"
#include "LFOSCommon.h"

using namespace std;

CameraInterface::CameraInterface()
{
	timeBetweenFrames = 0;
	timeOfLastCapture = 0;
	fdCamera = -1;
}


void CameraInterface::SetTimeBetweenFrames(unsigned int milliseconds)
{
	timeBetweenFrames = milliseconds;
}

unsigned int CameraInterface::GetTimeBetweenFrames()
{
	return timeBetweenFrames;
}


unsigned char* CameraInterface::CaptureFrame()
{
	unsigned char buffer[2*PIXELS];
    unsigned long timeMilliSeconds;
	struct timeval currentTime;

	gettimeofday(&currentTime, 0);


    timeMilliSeconds = currentTime.tv_sec*1000 + currentTime.tv_usec/1000;

    if((timeMilliSeconds - timeOfLastCapture) >= timeBetweenFrames)
	{

		read(fdCamera, buffer, 2*PIXELS);

		//Rotate image 180 degrees and store gray scale (first PIXEL bytes of array)
		for(int i = 0; i < PIXELS; ++i)
		{
			frameData[PIXELS-1-i] = buffer[i];
		}

        timeOfLastCapture = timeMilliSeconds;

		return frameData;
	}
	else
		return 0;
}

bool CameraInterface::Initialize(unsigned int time)
{
    //struct v4l2_capability cap;
	struct v4l2_format fmt;
	char fourcc[5]={0,0,0,0,0};
	unsigned char buffer[2*PIXELS];

    memset(&fmt, 0, sizeof(fmt));

    fmt.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = COLS;
    fmt.fmt.pix.height= ROWS;

    fdCamera = open("/dev/video0", O_RDWR);

	//ioctl(fd, VIDIOC_QUERYCAP, &cap);

	ioctl(fdCamera, VIDIOC_S_FMT, &fmt);
	//memmove(fourcc,&fmt.fmt.pix.pixelformat,4);

	timeBetweenFrames = time;

	//Read one frame since first frame is always garbage
	read(fdCamera, buffer, 2*PIXELS);

    return fdCamera != -1;
}

